#ifndef __MSD_H_
#define __MSD_H_



namespace math 
{
// numeric integraton. 
	template <typename value_type, unsigned dim >
		struct state  {};


	struct integration_tag {};
	struct euler_intergration : public integration_tag {};
	struct runge_kutta_integration : public integration_tag {};


	template <typename vector_, typename tag_type = euler_intergration>
		struct integration {};


	template <typename vector_ >
		struct integration <vector_, euler_intergration>
	{
		typedef vector_ vector_type ;
		typedef typename vector_type::value_type  value_type; 
	
		template <typename unary_function>
			vector_type operator () (const vector_type & xn, const value_type & h, unary_function fun)
		{
			return xn + fun(xn) * h;
		}
	};
// runge_kutta integration
	template <typename vector_>
		struct integration <vector_, runge_kutta_integration>
	{
		typedef vector_ vector_type ;
		typedef typename vector_type::value_type  value_type; 

//			k1 = fun (xn)
			// x is the offset 
			// x' = v 
			// v' = ( - ks * x - kd * v ) / m;

	
		template <typename binary_function>
			vector_type operator () (const vector_type & xn, const value_type & tn, const value_type & h, binary_function  fun)
		{
			vector_type k1 = fun (tn, xn) ;
			vector_type k2 = h * fun (tn + value_type(1.0/2.0) * h, xn + value_type (1.0/2.0) * k1);
			vector_type k3 = h * fun (tn + value_type(1.0/2.0) * h, xn + value_type (1.0/2.0) * k2);
			vector_type k4 = h * fun (tn + h , xn + k3) ;
			return xn + value_type (1.0/6.0) * (k1 + 2.0 * k2 + 2.0 * k3 + k4); 
		}
	};

}

namespace math 
{

	template <typename value_, unsigned dim> class MSDMeshFace ;
	template <typename value_, unsigned dim> class MSDMeshVertex ;
	template <typename value_, unsigned dim> class MSDMeshEdge ;
	template <typename value_, unsigned dim> class MSDMeshHalfEdge ;

	template <typename Value , unsigned dim> struct msd_mesh_traits  ;
	template <typename value> struct msd_mesh_traits <value, 3> : public mesh_traits <value, 3> 
	{
		typedef MSDMeshFace <value,3 >     FaceType ;
		typedef MSDMeshVertex <value,3 >   VertexType ;
		typedef MSDMeshEdge <value, 3>     EdgeType ; 
		typedef MSDMeshHalfEdge <value,3 > HalfEdgeType ;
		//typedef HalfEdge<value, 3> HalfEdgeType ;
	} ;

	template <typename value_, unsigned dim> class MSDMeshFace : public Face <value_, dim >
	{
	public :
		typedef value_ value_type; 
		typedef vector <value_type, dim> vector_type ;
		typedef vector_type point_type ;
		typedef typename Vertex<value_, dim>::VVIter VVIter ;
		typedef typename Face<value_, dim>::FVIter   FVIter ;
		typedef typename msd_mesh_traits<value_type, dim>::VertexType   vertex_type ;
		typedef typename msd_mesh_traits<value_type, dim>::HalfEdgeType halfedge_type ;
		typedef typename msd_mesh_traits<value_type, dim>::FaceType     face_type ;
		typedef math::HierarchyMesh<math::mesh_traits <double, 3> >     hierarchy_mesh_type ;
		typedef math::Segment<value_type, 3>                            segment_type ;
		typedef math::Polygon<value_type, 3>                            polygon_type ;

	public:
		void face_to_polygon (polygon_type & pol)
		{
			std::vector<point_type *> v_set; 
			this->Face2VertexCoord(v_set) ;
			for (unsigned i = 0; i < v_set.size(); ++i) pol.append_vertex ( * v_set[i]);
		}

		bool is_collide (face_type * & face)
		{
			polygon_type pol ;
			face_to_polygon (pol); 
			std::vector<unsigned> v_face_id ;

			if ( mp_model->intersect (pol, v_face_id))
			{
				face = (face_type *)mp_model->faces[v_face_id[0]] ;
				return true; 
			}

			return false; 
		}

		void collide_response (face_type * face)
		{
			for (FVIter iter = this->fv_begin(); iter != this->fv_end(); ++iter)
			{
				((vertex_type *)(*iter))->response_state(face); 
				//((vertex_type *)(*iter))->set_velocity (((vertex_type *)(*iter))->get_velocity () * (-0.1) ) ;
			}
		}
		void set_model (hierarchy_mesh_type * pmodel)
		{
			this->mp_model = pmodel ;
		}

	protected:
		hierarchy_mesh_type * mp_model ;
	}; 

	template <typename value_, unsigned dim> class MSDMeshEdge : public Edge <value_, dim >
	{		
	}; 


	template <typename value_, unsigned dim> class MSDMeshHalfEdge : public HalfEdge <value_, dim >
	{
	public:
		virtual void construct () 
		{
			update_edge_vector (); 
//			std::cout<<"edge_vector is "<<this->m_edge_vector <<std::endl;
			update_edge_length(); 
			this->m_init_edge_length = this->m_current_length ;
//			std::cout<<"edge_length is "<<this->m_init_edge_length <<std::endl;
		}
	protected :
		typedef value_ value_type ;
		typedef vector <value_type, dim> vector_type ;
		typedef vector_type point_type ;
		typedef typename msd_mesh_traits<value_type, dim>::VertexType vertex_type ;
		typedef typename msd_mesh_traits<value_type, dim>::HalfEdgeType halfedge_type ;
		
		
	public :
		void update () 
		{
			update_edge_vector (); 
			update_edge_length(); 
		}

		// We compute this force with respect to the from_v vertex
		// 
		// @pre update(); 
		//
		vector_type get_spring_force () const 
		{ 
			return m_edge_vector * (m_current_length - m_init_edge_length) * value_type (m_ks);
		}

		vector_type get_damper_force (vertex_type * v) const 
		{ 
			return m_edge_vector * (v->get_velocity() * m_edge_vector )  * value_type(-1.0) * value_type (m_kd)  ; 
		}

		value_type get_edge_length() const {return this->m_current_length ;}		
	protected :

		void get_vertices (vertex_type * & v1, vertex_type * & v2)
		{
			v1 = (vertex_type *) this->from_v ;
			v2 = (vertex_type *) this->next->from_v ;
		}
		// start from from_v to next->from_v
		vector_type update_edge_vector ()
		{
			vertex_type * v1, * v2 ;
			get_vertices (v1, v2) ;
			m_edge_vector = (v2->m_coord - v1->m_coord).normalize() ;
		}
		void update_edge_length ()
		{
			vertex_type * v1, * v2 ;
			get_vertices (v1, v2) ;
			m_current_length = distance (v1->m_coord, v2->m_coord);
		}

		static constexpr value_type m_ks  = 1000.0; // spring parameter ;
		static constexpr value_type m_kd  = 10.0; // damper parameter ; 
		value_type m_current_length; 
		value_type m_init_edge_length ; 
		vector_type m_edge_vector  ;

	}; 


	template <typename value_, unsigned dim> class MSDMeshVertex : public Vertex <value_, dim>
	{
	protected :
		typedef value_ value_type; 
		typedef vector <value_type, dim> vector_type ;
		typedef vector_type point_type ;
		typedef typename Vertex<value_, dim>::VVIter VVIter ;
		typedef typename Vertex<value_, dim>::VFIter VFIter ;
		typedef typename msd_mesh_traits<value_type, dim>::VertexType   vertex_type ;
		typedef typename msd_mesh_traits<value_type, dim>::HalfEdgeType halfedge_type ;
		typedef typename msd_mesh_traits<value_type, dim>::FaceType     face_type ;
		typedef math::HierarchyMesh<math::mesh_traits <double, 3> >     hierarchy_mesh_type ;
		typedef math::Segment<value_type, 3>                            segment_type ;
		typedef math::Polygon<value_type, 3>                            polygon_type ;

	public :
		bool m_intersect ;
		vector_type & get_normal () 
		{
			return this->m_vertex_normal ;
		}
		MSDMeshVertex () : m_velocity (vector_type (0.0, 0.0, 0.0)), m_lock(false), m_intersect (false){}
/////////////////Initialization function//////////////////
		virtual void construct ()
		{
			this->m_old_coord = this->m_coord ;
			this->m_old_acc = vector_type (0.0, 0.0, 0.0);
			compute_vertex_normal(); 
		}
		void compute_vertex_normal ()
		{
			vector_type n (0.0f, 0.0f, 0.0f);
			VFIter vfiter = this->vfbegin_safe();
			do 
			{
				n = n + (*vfiter)->normal();
				++ vfiter;
			} while (vfiter != this->vfend_safe());
			this->m_vertex_normal = n.normalize(); 			
		}

		// Set the intersection solid model
		void set_model (hierarchy_mesh_type * pmodel)
		{
			this->mp_model = pmodel ;
		}

////////////////Collide Response Function///////////////////////////////////////////
////////////////This is designed from vertex intersects the solid model/////////////
		// pre: m_old_coord and m_coord must be upgraded. 
		bool is_collide (face_type * & face)
		{
			// initial condition. 
			if (distance2 (this->m_old_coord, this->m_coord) < 1e-10) return false; 

			segment_type seg (this->m_old_coord, this->m_coord); 
			std::vector<unsigned> v_face_id ;
			if (true == mp_model->intersect (seg, v_face_id))
			{
				this->m_intersect = true; 
				face = (face_type *) mp_model->faces[v_face_id[0]] ;
				return true; 				
			}
			else
			{
				this->m_intersect = false ;
			}
			return false ;

			/*
			if (true == mp_model->segment_pick (seg, v_face_id))
			{
				face = (face_type *) mp_model->faces[v_face_id[0]] ;
				return true; 				
			}
			*/
		}


		void collide_response (face_type * pface, value_type timestep, value_type & remain_time)
		{
		
			/*
			// Better response
			vector_type face_normal = pface->get_normal(); 
			face_normal.normalize(); 
			polygon_type pol ;
			face_to_polygon (pface, pol);
			value_type old_dis = std::abs (distance (this->m_old_coord, pol )); 
			value_type new_dis = std::abs (distance (this->m_coord, pol));
			value_type f = old_dis / (old_dis + new_dis) ;
			
			// change the velocity and the position // refer to bounce.h
			vector_type collide_velocity = this->m_velocity + f * timestep * this->m_acceleration ;
			vector_type old_velocity = this->m_velocity ;

			this->m_velocity = change_collide_velocity(collide_velocity, face_normal); 
			this->m_coord = this->m_old_coord + f * timestep * m_old_velocity ;
			remain_time = timestep * (1.0 - f );
			*/
            
			// Simple response
			restore_state ();
			/*
			this->m_velocity = this->m_old_velocity * (-0.1);
			this->m_coord    = this->m_old_coord ;
			this->m_acceleration = this->m_old_acc ;
			*/
		}

////////////////////Properties updating function//////////
		value_type get_mass () const {return this->m_mass ;}
		void set_mass (value_type m) {this->m_mass = m ;}

		vector_type get_velocity () const {return this->m_velocity;}
		void set_velocity (const vector_type & v) {	this->m_velocity = v ; }

		vector_type get_position () const {return this->m_coord ;}
		void set_position (const vector_type & p)  { this->m_coord = p ; }

		vector_type get_acceleration () const {return this->m_acceleration ;}
		void set_acceleration (const vector_type & a)  {this->m_acceleration = a ;}

		void decompose_velocity (const vector_type & velocity, const vector_type & normal, vector_type & vn, vector_type & vt)
		{
			vn =  ( velocity * normal ) * normal ;
			vt = velocity - vn ; 
		}

		vector_type change_collide_velocity (const vector_type & velocity, const vector_type & face_normal)
		{
			double epsi = 0.1; 
			double miu = 0.01; 
			vector_type vn, vt; 
			decompose_velocity (velocity, face_normal, vn, vt) ;
			vn = - value_type (epsi) * vn ;
			vt = (1.0 - miu) * vt; 
			return vn + vt; 
		}


		void response_state (face_type * collide_face) 
		{
			/*
			this->m_acceleration  = this->m_old_acc  ;
			this->m_velocity      = change_collide_velocity (this->m_old_velocity, collide_face->get_normal()) ;
			this->m_coord         = this->m_old_coord  ;
			*/
			
			this->m_acceleration  = this->m_old_acc  ;
			this->m_velocity      = this->m_old_velocity * (-0.1);
			this->m_coord         = this->m_old_coord  ;
			
		}
		void restore_state ()
		{
			this->m_acceleration  = this->m_old_acc  ;
			this->m_velocity      = this->m_old_velocity ;
			this->m_coord         = this->m_old_coord ;
		
		}
		void save_state () 
		{
			this->m_old_acc = this->m_acceleration ;
			this->m_old_velocity = this->m_velocity ;
			this->m_old_coord = this->m_coord ;
		}

		void lock() {this->m_lock = true ;}
		void unlock () {this->m_lock = false ;}
		bool is_lock() const { return this->m_lock ;}

	public :
		void update_acceleration ()
		{
//			std::cout<<"velocity "<<get_velocity()<<std::endl;
			vector_type force (0.0, 0.0, 0.0);
			force += get_spring_force (); 
			force += get_damper_force (); 
			force += get_gravity();

			this->m_acceleration  = force / this->m_mass; 
		}
		//( - ks * x - kd * v ) / m;
		vector_type fv (const value_type & tn, const vector_type & vn)
		{
			return this->m_acceleration ; 
		}

		// offset indicate the offset of t. Maybe t, or t/2. 
		void update_velocity (value_type offset) 
		{
			// integration ; eular ;
			// update_acceleration (); 
			this->m_velocity += this->m_acceleration * offset ;
		}
		
		// x' = v ;
		vector_type fx (const value_type & tn, const vector_type & xn)
		{
			return this->m_velocity ;
		}
		void update_position (value_type offset)
		{
			if (is_lock() == true) return ;
			// integration ; eular ;
			this->m_coord += this->m_velocity * offset ; // old velocity
		}


	public:
		vector_type a0, a1, a2, a3 ; // these are placed for runge kutta method. 
		vector_type x0, v0 ;         // save the original state of vertices. 
	protected : 
		// convert face to polygon 


		// This is a safe get_neighbors, i.e., allows open boundary vertex. 
		void get_neighbors (std::vector<vertex_type *> & v_vertex)
		{
			halfedge_type * iter = this->he; 
			halfedge_type * iter_end = iter; 
			// find head ;
			bool flag = false; 
			do 
			{
				if (iter->prev->flip == NULL)
				{
					flag = true; 
					break; 
				}
				iter = iter->prev->flip ;
			}
			while (iter != iter_end) ;

			if (flag)
			{
				v_vertex.push_back(iter->prev->from_v);
			}
			iter_end = iter ;
			do 
			{
				v_vertex.push_back(iter->next->from_v);
				if (iter->flip == NULL) break; 
				iter = iter->flip->next ;
			}while(iter != iter_end);
		}

// Hasn't been tested. 
		void get_effective_he (std::vector<halfedge_type *> & v_he)
		{
			halfedge_type * iter = (halfedge_type *)this->he; 
			halfedge_type * iter_end = iter; 
			// find head ;
			bool flag = false; 
			do 
			{
				if (iter->prev->flip == NULL)
				{
					flag = true; 
					break; 
				}
				iter = (halfedge_type *)iter->prev->flip ;
			}
			while (iter != iter_end) ;

			if (flag)
			{
				v_he.push_back((halfedge_type *)iter->prev);
			}
			iter_end = iter ;
			do 
			{
				v_he.push_back(iter);
				if (iter->flip == NULL) break; 
				iter = (halfedge_type *)iter->flip->next ;
			}while(iter != iter_end);
		}

		vector_type get_spring_force ()
		{
			vector_type force (0.0, 0.0, 0.0);
			std::vector<halfedge_type *> vhe; 
			get_effective_he (vhe);
			if (vhe[0]-> from_v != this)
			{
				force -= vhe[0]->get_spring_force();
			}
			else 
			{
				force += vhe[0]->get_spring_force(); 
			}
			for (unsigned i = 1; i < vhe.size(); ++i)
			{
				force+= vhe[i]->get_spring_force();
			}
			return force ;
		}

// fix this. 
		vector_type get_damper_force ()
		{
			vector_type force (0.0, 0.0, 0.0);
			std::vector<halfedge_type *> vhe; 
			get_effective_he (vhe);

			for (unsigned i = 0; i < vhe.size(); ++i)
			{
				force+= vhe[i]->get_damper_force(this); 
			}
			return force ;
		}


		vector_type get_gravity () const 
		{
			return this->m_mass * vector_type (0, 0, -9.8);
		}

//		integration<vector_type, runge_kutta_integration > integrator; 
		static constexpr value_type m_mass = 0.2 ; 
		static constexpr value_type epsi = 0.02 ; // bouncing coefficient; 0 \leq epsi \leq 1; 
		static constexpr value_type miu  = 0.05; // miu is horizontal friction coefficient; 

		vector_type m_velocity; 
		vector_type m_acceleration ; 

		hierarchy_mesh_type * mp_model ;

		
		// m_coord is the position ; 

		vector_type m_old_coord; 
		vector_type m_old_velocity ;
		vector_type m_old_acc ;
		bool m_lock ;
		
		vector_type m_vertex_normal ;

	};


	template<typename mesh_traits>
		class MSDMesh : public Mesh <mesh_traits>
	{
	public:
	   	typedef typename mesh_traits::ValueType              ValueType; 
		typedef typename mesh_traits::FaceType               FaceType;
		typedef typename mesh_traits::VertexType             VertexType;
		typedef typename mesh_traits::HalfEdgeType           HalfEdgeType ; 
		typedef typename mesh_traits::EdgeType               EdgeType ; 
		typedef typename mesh_traits::VectorType             VectorType ;
		typedef typename mesh_traits::PointType              PointType ;
//		typedef typename mesh_traits::MeshType               MeshType ;
		typedef typename Mesh<mesh_traits>::HalfEdgeIterator HalfEdgeIterator ;
		typedef typename Mesh<mesh_traits>::VertexIterator   VertexIterator ;
		typedef typename Mesh<mesh_traits>::FaceIterator     FaceIterator ;
		typedef Segment<ValueType, mesh_traits::dim>         SegmentType ;
		typedef Ray<ValueType, mesh_traits::dim>             RayType ;

		typedef math::HierarchyMesh<math::mesh_traits <ValueType, mesh_traits::dim> >  hierarchy_mesh_type ;

		typedef math::Mesh <mesh_traits>                            parent_type ;
		typedef AABBTreePoly <ValueType, mesh_traits::dim>          AABBTreePolyType; 
		typedef AABBTreePolyBuilder <ValueType, mesh_traits::dim>   AABBTreePolyBuilderType  ; 
		typedef AABBTreePolygon <ValueType, mesh_traits::dim>       AABBTreePolygonType ;
		typedef AABBTreeNode <ValueType, mesh_traits::dim>          AABBTreeNodeType ;

		typedef Box<ValueType, mesh_traits::dim>                    BoxType; 
		typedef Polygon<ValueType, mesh_traits::dim>                PolygonType; 
		typedef Sphere<ValueType, mesh_traits::dim>                 SphereType; 

		typedef typename AABBTreePolyType::PolygonColResult         PolygonColResult;
		typedef typename AABBTreePolyType::RayColResult             RayColResult;
		typedef typename AABBTreePolyType::SphereColResult          SphereColResult;
		typedef typename AABBTreePolyType::SegmentColResult         SegmentColResult;

	public: 

	MSDMesh(): mp_tree( NULL ) {}
		virtual ~MSDMesh() 
		{
			if (this->mp_tree != NULL)
				delete this->mp_tree; 
		}

		void set_model (hierarchy_mesh_type * pmodel) 
		{
			this->mp_model = pmodel ;
		}
/*
		AABBTreePolyType * build_hierarchy ()
		{
			if (this->mp_tree != NULL) delete this->mp_tree; 
			AABBTreePolyBuilderType builder ;
			this->mp_tree = builder.build_from_mesh ( (Mesh<mesh_traits > &) (* this), 1);
			return this->mp_tree; 
		}
		AABBTreePolyType * get_aabbtree() const 
		{
			return this->mp_tree; 
		}

		bool is_intersect (const PolygonType & polygon)
		{
			PolygonColResult result ;
			this->mp_tree->collideWithPolygon (polygon, result);
			return result.polys_.size() > 0; 
		}
		bool is_intersect (const SphereType & sphere)
		{
			SphereColResult result ;
			this->mp_tree->collideWithSphere (sphere, result);
			return result.polys_.size() > 0; 
		}
		bool intersect (const PolygonType & polygon, std::vector<unsigned > & v_face_id)
		{
			PolygonColResult result ;			
			this->mp_tree->collideWithPolygon (polygon, result);
			if ( result.polys_.size() == 0) return false; 
			for (unsigned i = 0; i < result.polys_.size(); ++i)
			{
				v_face_id.push_back(result.polys_[i]->get_face()->m_idx);
			} 
			return true; 
		}


		bool intersect (const SphereType & sphere, std::vector<unsigned > & v_face_id)
		{
			SphereColResult result ;			
			this->mp_tree->collideWithSphere (sphere, result);
			if ( result.polys_.size() == 0) return false; 
			for (unsigned i = 0; i < result.polys_.size(); ++i)
			{
				v_face_id.push_back(result.polys_[i]->get_face()->m_idx);
			} 
			return true; 
		}

		bool intersect (const SegmentType & seg, std::vector<unsigned> & v_face_id )
		{
			SegmentColResult result ;
			this->mp_tree->collideWithSegment (seg, result);
			if ( result.polys_.size() == 0) return false; 
			for (unsigned i = 0; i < result.polys_.size(); ++i)
			{
				v_face_id.push_back(result.polys_[i]->get_face()->m_idx);
			} 
			return true; 
		}

		bool intersect (const RayType & ray, std::vector<unsigned> & v_face_id )
		{
			RayColResult result ;
			this->mp_tree->collideWithSegment (ray, result);
			if ( result.polys_.size() == 0) return false; 
			for (unsigned i = 0; i < result.polys_.size(); ++i)
			{
				v_face_id.push_back(result.polys_[i]->get_face()->m_idx);
			} 
			return true; 
		}
		bool segment_pick (const SegmentType & seg, std::vector<unsigned> & v_face_id )
		{
			SegmentColResult result ;
			this->mp_tree->SegmentPick (seg, result);
			if ( result.polys_.size() == 0) return false; 
			for (unsigned i = 0; i < result.polys_.size(); ++i)
			{
				v_face_id.push_back(result.polys_[i]->get_face()->m_idx);
			} 
			return true; 
		}

*/
		// method = 1; eular 
		// method = 2; runge-kutta ;
		void update_model (unsigned method)
		{
			save_vertices_states(); 
			update_vertices_positon (method ) ;
			//collide_detection_vertex (); 
			collide_detection_face();
			update_face_normal ();
			update_vertex_normal();
		}

		virtual void construct ()
		{
			parent_type::construct(); 
			for (VertexIterator it = this->vertices.begin();
				 it != this->vertices.end(); ++it)
			{
				it->second->set_model (this->mp_model); // set model pointer
			}
			for (FaceIterator it = this->faces.begin(); it != this->faces.end(); ++it)
			{
				(*it)->set_model (this->mp_model);
			}
		} 

	protected:
		void update_vertices_positon (unsigned method)
		{
			if (method == 1)
				update_model_eular();
			else 
				update_model_runge_kutta();
		}

		void update_vertex_normal()
		{
			for (VertexIterator v_iter = this->vertices.begin(); v_iter != this->vertices.end(); ++v_iter )
			{
				v_iter->second->compute_vertex_normal();
			}			
		}
		void update_face_normal ()
		{
			FaceIterator f_iter ;
			for (f_iter = this->faces.begin(); f_iter != this->faces.end(); ++f_iter)
			{
				(*f_iter)->compute_normal();
			}
		}

		void save_vertices_states ()
		{
			for (VertexIterator v_iter = this->vertices.begin(); v_iter != this->vertices.end(); ++v_iter )
			{
				v_iter->second->save_state(); 
			}
		}

		void collide_detection_face ()
		{
			for (FaceIterator f_iter = this->faces.begin(); f_iter != this->faces.end(); ++f_iter)
			{
				FaceType * face ;
				if ( (*f_iter)->is_collide (face))
				{
					(*f_iter)->collide_response (face); 
				}
			}
		}
		void collide_detection_vertex ()
		{
			for (VertexIterator v_iter = this->vertices.begin(); v_iter != this->vertices.end(); ++v_iter )
			{
				FaceType * pface ;
				ValueType remain_time; 
				if (v_iter->second->is_collide( pface))
				{
					v_iter->second->collide_response(pface, this->m_timestep, remain_time);
				}

			}			
		}
		void update_model_runge_kutta()
		{
			static unsigned count = 0; 
			std::cout<<"Frame "<<count++<<std::endl;
            // save original state 
			VertexIterator v_iter ; 
			for (v_iter = this->vertices.begin(); v_iter != this->vertices.end(); ++v_iter )
			{
				v_iter->second->x0 = v_iter->second->get_position();
				v_iter->second->v0 = v_iter->second->get_velocity();
			}

			for (v_iter = this->vertices.begin(); v_iter != this->vertices.end(); ++v_iter )
			{
				v_iter->second->update_acceleration () ;
				v_iter->second->a0 = v_iter->second->get_acceleration(); 

				v_iter->second->update_velocity(this->m_timestep / 2.0) ;
				v_iter->second->update_position(this->m_timestep / 2.0) ;


				//	v_iter->second->set_position (v_iter->second->x0)  ;
				//  v_iter->second->set_velocity (v_iter->second->v0)  ;
			}

			for (HalfEdgeIterator it = this->hash_he.begin(); it != this->hash_he.end(); ++it)
			{
				it->second->update();
			}

			for (v_iter = this->vertices.begin(); v_iter != this->vertices.end(); ++v_iter )
			{
				v_iter->second->update_acceleration () ;
				v_iter->second->a1 = v_iter->second->get_acceleration(); 
				
				v_iter->second->set_position (v_iter->second->x0)  ;
				v_iter->second->set_velocity (v_iter->second->v0)  ;

				v_iter->second->update_velocity(this->m_timestep / 2.0) ;
				v_iter->second->update_position(this->m_timestep / 2.0) ;


			}

			for (HalfEdgeIterator it = this->hash_he.begin(); it != this->hash_he.end(); ++it)
			{
				it->second->update();
			}

			for (v_iter = this->vertices.begin(); v_iter != this->vertices.end(); ++v_iter )
			{
				v_iter->second->update_acceleration () ;
				v_iter->second->a2 = v_iter->second->get_acceleration(); 
				
				v_iter->second->set_position (v_iter->second->x0)  ;
				v_iter->second->set_velocity (v_iter->second->v0) ;

				v_iter->second->update_velocity(this->m_timestep ) ;
				v_iter->second->update_position(this->m_timestep ) ;

			}

			for (HalfEdgeIterator it = this->hash_he.begin(); it != this->hash_he.end(); ++it)
			{
				it->second->update();
			}

			for (v_iter = this->vertices.begin(); v_iter != this->vertices.end(); ++v_iter )
			{
				v_iter->second->update_acceleration () ;
				v_iter->second->a3 = v_iter->second->get_acceleration(); 
			}

			for (v_iter = this->vertices.begin(); v_iter != this->vertices.end(); ++v_iter )
			{

				v_iter->second->set_position (v_iter->second->x0)  ;
				v_iter->second->set_velocity (v_iter->second->v0) ;
				v_iter->second->set_acceleration ( 1.0 / 6.0 * (v_iter->second->a0 
													   + v_iter->second->a3
													   + 2.0 * v_iter->second->a1
													   + 2.0 * v_iter->second->a2));
				v_iter->second->update_velocity(this->m_timestep ) ;
				v_iter->second->update_position(this->m_timestep ) ;

			}

			for (HalfEdgeIterator it = this->hash_he.begin(); it != this->hash_he.end(); ++it)
			{
				it->second->update();
			}
		}

		void update_model_eular ()
		{
			static unsigned count = 0; 
			std::cout<<"Frame "<<count++<<std::endl;
			VertexIterator v_iter ; 
			for (v_iter = this->vertices.begin(); v_iter != this->vertices.end(); ++v_iter )
			{
				v_iter->second->update_acceleration () ;
				v_iter->second->update_position(this->m_timestep) ;
				v_iter->second->update_velocity(this->m_timestep) ;
			}

			for (HalfEdgeIterator it = this->hash_he.begin(); it != this->hash_he.end(); ++it)
			{
				it->second->update();
			}
		}

	private :
		AABBTreePolyType * mp_tree; 
		hierarchy_mesh_type * mp_model ;
		static constexpr ValueType m_timestep = 0.005; 
	};
};



#endif
